import cv2
import numpy as np
import pandas as pd
from scipy.optimize import least_squares
import matplotlib.pyplot as plt
import math
import serial
import serial.tools.list_ports
from time import sleep
import threading, time
import atexit
from collections import defaultdict
from sklearn.linear_model import RANSACRegressor
# from Thread import event
import binascii
import os
import signal
import socket
import sys

# 打开摄像头，如果电脑还有其他摄像头，此参数设置为1或者2
# cap = cv2.VideoCapture(0)
# a, b = cap.read()

ransac = RANSACRegressor()

@atexit.register
def exit_callback():
    # 释放摄像头资源
    cap.release()
    cv2.destroyAllWindows()
    print("主程序终止!摄像头资源已释放。")


# # 注册回调函数
# atexit.register(exit_callback)


# 设置校准点的个数
SetCalPonitNum = 10
# 初始化计算实际曲线的buffer
ActualAngle = np.zeros((8, SetCalPonitNum))  #初始化要用到的数据
startAngle = 175
ThumbsCal = 0  #大拇指标定标志位
StandardAngle = [175, 170, 160, 150, 140, 130, 120, 110, 100, 90]  #标准的矫正点位
Angle = []
for i in range(SetCalPonitNum):
    ActualAngle[0, i] = startAngle

    if i < 2:
        startAngle = startAngle - 5
    else:
        startAngle = startAngle - 10


# 校准完成的手指个数
FinishCal = 1
# 接收电缸行程
EleAxisStroke = 0

# path = '1.jpg'
dst_path = r'D:\Project\PythonProject\HandCal\img/'
Finename = r'D:\Project\PythonProject\HandCal\img/'

image_count = 1   # 用于图片序号的计数器


# 计算与竖直方向的夹角
def calculate_angle_from_slope(slope):
    # 计算夹角（弧度）
    angle_rad = math.atan(slope)
    # 将弧度转换为角度
    angle_deg = - math.degrees(angle_rad) + 90

    return angle_deg


# 计算两条线之间的夹角
def calculate_angle_between_lines(slope1, slope2):
    # 计算夹角（弧度）
    angle_rad = math.atan(abs((slope2 - slope1) / (1 + slope1 * slope2)))
    # 将弧度转换为角度
    angle_deg = math.degrees(angle_rad)
    angle_deg = 180 - angle_deg

    return angle_deg


# 定义一个对检测结果的判定，正确返回1，错误返回0
def check_result(num):
    if num == 2:
        return 1
    else:
        return 0
    # try:
    #     # 使用assert语句检查条件是否为真
    #     assert num <= 2, "检测错误：环境干扰较多"
    # except AssertionError as e:
    #     # 捕获AssertionError异常并处理
    #     print(f"错误信息：{e}")
    #     # 抛出异常，终止程序
    #     raise

#当四个其余手指标定完成之后，进行大拇指标定
def Change2ThumbsCal():
    global ThumbsCal
    global SetCalPonitNum
    global StandardAngle
    if ThumbsCal == 1:
        SetCalPonitNum = 8
        startAngle = 165
        StandardAngle = [165, 160, 155, 150, 145, 140, 135, 130]
        print(f'更改大拇指标定点位为:{StandardAngle}')
        Angle = []
        for i in range(SetCalPonitNum):
            ActualAngle[0, i] = startAngle
            startAngle = startAngle - 5
        



# 提取红色线条区域，计算角度，复用函数
def RedLineExact(path):
    global image_count
    global Angle
    img = cv2.imread(path)
    # 获取原始图像的宽高
    original_height, original_width = img.shape[:2]
    # 计算比例
    scale = min(1920 / original_width, 1280 / original_height)

    # 计算新宽高
    new_width = int(original_width * scale)
    new_height = int(original_height * scale)

    # resize原始图像，复原宽高比
    img = cv2.resize(img, (new_width, new_height))

    img_HSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)  #HSV空间处理图像相较简单

    # 定义红色范围
    lower_red = np.array([0, 65, 65])
    upper_red = np.array([10, 255, 255])
    # 创建红色蒙版
    red_mask0 = cv2.inRange(img_HSV, lower_red, upper_red)

    lower_red = np.array([160, 65, 65])
    upper_red = np.array([180, 255, 255])
    # 创建红色蒙版
    red_mask1 = cv2.inRange(img_HSV, lower_red, upper_red)

    # 合并红色蒙版
    red_mask = red_mask0 + red_mask1

    # 腐蚀 去除干扰
    kernel_erode = np.ones((8, 8), np.uint8)
    red_mask_erode = cv2.erode(red_mask, kernel_erode, iterations=1)
    # 膨胀 使线条连续
    kernel = np.ones((2, 2), np.uint8)
    red_mask_dilated = cv2.dilate(red_mask_erode, kernel, iterations=2)

    # 找膨胀之后红线的外框,
    contours, _ = cv2.findContours(red_mask_dilated, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    num_of_line = len(contours)
    # 判断红色线条是否提取正确
    check_res = check_result(num_of_line)
    print(f'线条个数为：{num_of_line}')

    # 找红线的外框外框的中心点
    screw_centers = []
    LineEquations = []
    LineXCoordinate = []

    if check_res:
        for contour in contours:
            x, y, w, h = cv2.boundingRect(contour)

            cv2.rectangle(img, (x, y), (x + w, y + h), (0, 255, 0), 2)  #画绿色外框

            center_x = x + w // 2
            center_y = y + h // 2   #获取外框中心坐标
            screw_centers.append((center_x, center_y))
            LineXCoordinate.append(center_x)
            # 截取红线外框ROI,像素下标会从0开始，后续计算需要做补偿
            # 查找二值图像中白色像素的坐标
            # white_pixels第一列为第几行，对应原图的y，第二列为第几列，对应原图的x
            white_pixels = np.column_stack(np.where(red_mask_erode[y:y + h, x:x + w] > 0))  # y:x 第一个是纵向，第二个是横向
            # 尺寸回溯
            white_pixels[:, 0] = white_pixels[:, 0] + y
            white_pixels[:, 1] = white_pixels[:, 1] + x
            white_pixels_ori_coordinate = white_pixels
            # 进行直线拟合，最少需要两个点
            if len(white_pixels_ori_coordinate) >= 2:
                # RANSAC (Random Sample Consensus)法，随机抽样一致法，相对最小二乘法是一种优化，默认迭代1000次，且为线性拟合
                ransac.fit(white_pixels_ori_coordinate[:, 1].reshape(-1, 1),
                           white_pixels_ori_coordinate[:, 0])  # 第一个参数将x展开为nx1数组，第二个为y
                inlier_mask = ransac.inlier_mask_
                outlier_mask = np.logical_not(inlier_mask)
                # 输出拟合结果
                k_ransac = ransac.estimator_.coef_[0]  # 获取斜率
                b_ransac = ransac.estimator_.intercept_  # 获取截距
                line_params = [k_ransac, b_ransac]
                LineEquations.append(line_params)
                # # 使用最小二乘法拟合直线
                # initial_params = [0, 0]  # 初始参数
                # result = least_squares(loss_function, initial_params, args=(white_pixels_ori_coordinate[:, 1],
                #                         white_pixels_ori_coordinate[:, 0]))
                # # 输出拟合结果
                # k_opt, b_opt = result.x
                # print(f"Optimized parameters: k = {k_opt}, b = {b_opt}")
                # LineEquations.append(result.x)

                # # 一次多项式拟合，直线                x                               y                              次数
                # line_params = np.polyfit(white_pixels_ori_coordinate[:, 1], white_pixels_ori_coordinate[:, 0], 1)
                # LineEquations.append(line_params)
                # print("line_params:", line_params)
                # 带入x坐标范围，获取拟合的直线方程算出来的值
                # line_y = np.polyval(result.x, np.arange(x, x + w))
                line_y = np.polyval(line_params, np.arange(x, x + w))
                # print(f"Line coordinates: x1={x}, y1={int(line_y[0])}, x2={x + w - 1}, y2={int(line_y[-1])}")

                for i in range(len(line_y) - 1):
                    cv2.line(img, (x + i, int(line_y[i])), (x + i + 1, int(line_y[i + 1])), (255, 0, 0), 2)

            cv2.circle(img, (center_x, center_y), radius=5, color=(0, 0, 255), thickness=-1)  # -1表示填充圆
            # print(f"红线外接框中心坐标为:{center_x},{center_y}")

        # 计算关节夹角
        # print("LineXCoordinate:", LineXCoordinate)
        minIndex = LineXCoordinate.index(min(LineXCoordinate))

        if minIndex == 0:
            Angle1 = calculate_angle_from_slope(LineEquations[0][0])
            Angle2 = calculate_angle_between_lines(LineEquations[0][0], LineEquations[1][0])
            cv2.putText(img, f"Agnle1:{Angle1:.2f} degrees", (screw_centers[0][0], screw_centers[0][1]),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
            cv2.putText(img, f"Agnle2:{Angle2:.2f} degrees", (screw_centers[1][0] + 50, screw_centers[1][1] + 50),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        else:
            Angle1 = calculate_angle_from_slope(LineEquations[1][0])
            Angle2 = calculate_angle_between_lines(LineEquations[1][0], LineEquations[0][0])
            cv2.putText(img, f"Agnle1:{Angle1:.2f} degrees", (screw_centers[1][0] + 20, screw_centers[1][1] + 20),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
            cv2.putText(img, f"Agnle2:{Angle2:.2f} degrees", (screw_centers[0][0] + 50, screw_centers[0][1] + 50),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        print("角度1：", Angle1)
        print("角度2：", Angle2)
        if image_count == SetCalPonitNum * 2 + 1:
            print(ActualAngle)

        client_socket.send('O'.encode('utf-8'))
        print("处理完毕，进行拍图")
        # if ser.is_open:
        #     ser.write('O'.encode('utf-8'))  # 编码 发送O--OK，ascii为79
        #     print("处理完毕，进行拍图")
        # else:
        #     print("发送失败！串口通信出现问题")

        Angle = [Angle1, Angle2]
    else:
        Angle = [0,0]
        client_socket.send('E'.encode('utf-8'))
        image_count = image_count - 1
        print(f"反馈的错误码为：E")
        print(f"图像错误之后的count为：{image_count}")
        # if ser.is_open:
        #     ser.write('E'.encode('utf-8'))  # 编码 发送O--OK，ascii为79
        #     image_count = image_count - 1
        #     print(f"反馈的错误码为：E")
        #     print(f"图像错误之后的count为：{image_count}")
        #
        # else:
        #     print("发送失败！串口通信出现问题")

    return Angle, img, red_mask_dilated, red_mask_erode


# 存储每次计算的手指角度，以及电缸行程
def StoreAcualAngle(angle):
    global image_count
    global EleAxisStroke
    index = 1
    if image_count <= SetCalPonitNum + 1:
        ActualAngle[index, image_count - 2] = angle
        ActualAngle[index + 4, image_count - 2] = EleAxisStroke
    elif image_count > SetCalPonitNum + 1 & image_count < SetCalPonitNum * 2 + 2:
        index = 2
        ActualAngle[index, image_count - SetCalPonitNum - 2] = angle
        ActualAngle[index + 4, image_count - SetCalPonitNum - 2] = EleAxisStroke
    elif image_count >= SetCalPonitNum * 2 + 2:
        index = 1
        print('超出设置的校准点个数了，检查配置信息')


# 对图像进行线条检测，计算角度
def LineDetect(path):
    global image_count
    global Angle
    [Angle, img, red_mask_dilated, red_mask_erode] = RedLineExact(path)
    if Angle[0] != 0:
        StoreAcualAngle(Angle[0])


    # cv2.namedWindow('img', cv2.WINDOW_NORMAL)  # 解决显示过大，显示不全的问题，解决图片分辨率大于电脑分辨率问题
    # cv2.namedWindow('red_mask_dilated', cv2.WINDOW_NORMAL)  # 解决显示过大，显示不全的问题，解决图片分辨率大于电脑分辨率问题
    # cv2.namedWindow('red_mask_erode', cv2.WINDOW_NORMAL)  # 解决显示过大，显示不全的问题，解决图片分辨率大于电脑分辨率问题
    # cv2.imshow('img', img)
    # cv2.imshow('red_mask_dilated', red_mask_dilated)
    # cv2.imshow('red_mask_erode', red_mask_erode)
    # cv2.waitKey(0)





# 打开摄像头，拍摄并存储图像
def capture_and_save_image(image_count):
    # try:
    global Finename
    # 读取摄像头帧
    ret, frame = cap.read()

    # 如果成功读取帧
    if ret:
        # # 显示帧
        # cv2.imshow('Frame', frame)
        # 生成文件名
        filename = dst_path + f"{image_count}.jpg"
        # 保存图像
        Finename = filename # dst_path + f"{image_count - 1}.jpg"
        cv2.imwrite(filename, frame)
        print(len(frame.shape))
        print(f"Image saved as {Finename}")
        return 0x4F

    # finally:
    #     # 释放摄像头资源
    #     cap.release()
    #     cv2.destroyAllWindows()


# 接收串口发送的指令
def RecvSerialCmd():
    global image_count
    global EleAxisStroke
    try:
        while True:
            # 从串口读取一个字节数据
            data = []
            # data = ser.read_until(b'\r\n')
            # 从客户端接收数据
            data = client_socket.recv(3)
            print(f"Received data from client: {data}")
            print(data)
            print(data[0])
            print(data[1])
            print(data[2])
            if data[0] == 0x42:  # 标定指令 B
                res = capture_and_save_image(image_count)
                image_count += 1
                print(f"拍图存储之后的count为：{image_count}")
                # print('data[0]', data[0])
                EleAxisStroke = (data[1] << 8) + data[2]
                print(EleAxisStroke)
                event.set()  # 设置事件，一旦set,那么读者wait就有返回了，读者可以继续运行了
            if data[0] == 0x56:  # 验证指令 V
                res = capture_and_save_image(image_count)
                image_count += 1
                print(f"拍图存储之后的count为：{image_count}")
                event_Verification.set()
            break
    except KeyboardInterrupt:
        print("尝试CTRL+C终止")


# 处理图像的任务
def PocessPicture():
    global image_count
    event_CalCompensation.clear()
    while True:
        if event.is_set():
            print("准备处理图像")
            LineDetect(Finename)
            # 读取count值，如果是20 ，弯曲时 十张图 ，返程伸直时拍摄十张 两次数据做个平均，考虑回程差问题
            # print('image_count的值为：',image_count)
            if image_count == SetCalPonitNum * 2 + 1:
                event_CalCompensation.set()  # 设置计算补偿的事件
            time.sleep(2)
            event_CalCompensation.clear()  # 清除设置补偿的事件

        else:
            print("新图还未保存")
            event.wait()  # 那么就等着咯


# 存储图像的任务
def StorePicture():
    global image_count
    global Finename
    event.clear()
    event_Verification.clear()
    while True:
        RecvSerialCmd()
        # print(Finename)
        time.sleep(2)
        event.clear()  # 清除事件
        event_Verification.clear()


# 计算实际曲线 并将参数发送给灵巧手
def CalCompensation():
    global Finename
    global FinishCal
    global image_count
    global EleAxisStroke
    while True:
        if event_CalCompensation.is_set():
            print('灵巧手正在进行校准')

            for i in range(SetCalPonitNum):  # 对弯曲和伸直时的数据进行一次平均，考虑回程差进去
                ActualAngle[3, i] = (ActualAngle[1, i] + ActualAngle[2, SetCalPonitNum-1-i])/2
                ActualAngle[7, i] = (ActualAngle[5, i] + ActualAngle[6, SetCalPonitNum - 1 - i]) / 2

            polyfit_params = np.polyfit( ActualAngle[3, :],ActualAngle[7, :]/1000, 3)  # 拟合三次曲线
            ActualAngle[4, 0] = polyfit_params[0]
            ActualAngle[4, 1] = polyfit_params[1]
            ActualAngle[4, 2] = polyfit_params[2]
            ActualAngle[4, 3] = polyfit_params[3]
            csv_ActualAngle = pd.DataFrame(ActualAngle)
            csv_path = f'csv_ActualAngle{FinishCal}.csv'
            if os.path.exists(csv_path):
                # 删除文件
                os.remove(csv_path)
                print(f"{csv_path} 已被删除。")
            if ~os.path.exists(csv_path):
                csv_ActualAngle.to_csv(f'csv_ActualAngle{FinishCal}.csv')
                print("polyfit_params:", polyfit_params)
                sleep(0.1)
                client_socket.send(f'OK{FinishCal}'.encode("utf-8"))
                print(f"手指{FinishCal}处理完毕，进行拍图验证")
                image_count = 1  # 重置拍图个数，进行下一个手指标定
                sleep(2)
                for value in polyfit_params:
                    value_str = str(value)
                    print(value_str)
                    print(value_str.encode('utf-8'))
                    client_socket.send(value_str.encode('utf-8'))
                    sleep(2)
                # if FinishCal == 4:
                #     print("全部处理完毕")

                # if (ser.is_open):
                #     ser.write(f'Ok{FinishCal}'.encode('utf-8'))  # 编码 发送O--OK，ascii为79
                #     print(f"手指{FinishCal}处理完毕，进行拍图")
                #
                #     image_count = 1  # 重置拍图个数，进行下一个手指标定
                #     for value in polyfit_params:
                #         # 将float类型转换为字节
                #         # 由于小数点后有8位，我们将其转换为整数再发送
                #         value_int = int(value * 100000000)  # 将小数放大100,000,000倍转为整数
                #         value_bytes = value_int.to_bytes(4, byteorder='little')  # 使用4字节发送数据
                #         ser.write(value_bytes)
                #         time.sleep(0.1)  # 可选的延迟，根据需要调整
                #     if FinishCal == 4:
                #         print("全部处理完毕")
                # else:
                #     print("发送失败！串口通信出现问题")

            time.sleep(2)
        else:
            print('灵巧手在等待校准')
            event_CalCompensation.wait()  # 等待校准


# 利用标定完后的参数进行闭环验证 的任务
def AngleVerification():
    global Angle
    global FinishCal
    global image_count
    global Finename
    global ThumbsCal
    cameraAngle = list()
    while True:
        if event_Verification.is_set():

            [Angle, img, red_mask_dilated, red_mask_erode] = RedLineExact(Finename)
            print(f"angle:{Angle[0]}")
            if(Angle[0]!= 0):
                cameraAngle.append(Angle[0])
                print(f'cameraAngle:{cameraAngle}')
            okAngleNum = 0
            if image_count - 1 == SetCalPonitNum:
                # 用拍到的角度和标准角度进行对比
                for i in range(SetCalPonitNum):
                    res = abs(StandardAngle[i]-cameraAngle[i])
                    print(f'res:{res}')
                    if res < 3:
                        okAngleNum = okAngleNum + 1
                        print(f'满足精度要求的手指角度个数为：{okAngleNum}')

                if okAngleNum < SetCalPonitNum:
                    print(f'手指{FinishCal}验证失败，请重新标定')
                    image_count = 1
                    client_socket.send(f'VE{FinishCal}'.encode('utf-8'))
                    cameraAngle = []
                    # if (ser.is_open):
                    #     ser.write(f'VE{FinishCal}'.encode('utf-8'))  # 编码 发送O--OK，ascii为79

            if image_count - 1 > SetCalPonitNum:
                print(f'手指{FinishCal}验证紊乱，请重新验证')
                okAngleNum = 0
                client_socket.send(f'VE{FinishCal}'.encode('utf-8'))
                image_count = 1
                cameraAngle = []
                # if (ser.is_open):
                #     ser.write(f'VE{FinishCal}'.encode('utf-8'))  # 编码 发送O--OK，ascii为79
                #     image_count = 1
            if okAngleNum == SetCalPonitNum:
                # 若对比成功，反馈VOK，进行下一个手指的标定
                client_socket.send(f'VO{FinishCal}'.encode('utf-8'))
                print(f'手指{FinishCal}验证成功')
                cameraAngle = []
                FinishCal = FinishCal + 1  # 闭环验证成功
                image_count = 1

                if FinishCal == 5:
                    ThumbsCal = 1
                    Change2ThumbsCal()

                # if (ser.is_open):
                #     ser.write(f'VO{FinishCal}'.encode('utf-8'))  # 编码 发送O--OK，ascii为79
                #     print(f'手指{FinishCal}验证成功')
                #     FinishCal = FinishCal + 1  # 闭环验证成功
                #     image_count = 1

            time.sleep(2)
        else:
            event_Verification.wait()
            print('等待验证矫正结果')


if __name__ == "__main__":

    # ports_list = list(serial.tools.list_ports.comports())
    # if len(ports_list) <= 0:
    #     print("无串口设备。")
    # else:
    #     print("可用的串口设备如下：")
    #     for comport in ports_list:
    #         print(list(comport)[0], list(comport)[1])
    # # 打开串口
    # ser = serial.Serial('COM8', 115200)  # 请根据实际情况更改COM端口和波特率
    # if ser.is_open:
    #     print("打开串口成功！")
    # else:
    #     print("打开串口失败！")
    cap = cv2.VideoCapture(0)
    a, b = cap.read()
    # 创建 socket 对象
    server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    host = '192.168.1.175'  # 监听所有的公开的 IP 地址
    port = 65525  # 非特权端口
    # 绑定主机和端口
    server_socket.bind((host, port))

    # 开始监听
    server_socket.listen(5)
    print(f"Server listening on {host}:{port}...")

    while True:
        # 等待客户端连接
        client_socket, addr = server_socket.accept()
        print(f"Connection from {addr} has been established!")
        break

    event = threading.Event()  # 创建事件对象，触发处理图像
    event_CalCompensation = threading.Event()  # 创建事件对象，触发计算补偿值并发送
    event_Verification = threading.Event() # 创建事件对象，触发手指参数更新

    t1 = threading.Thread(target=StorePicture)
    t2 = threading.Thread(target=PocessPicture)
    t3 = threading.Thread(target=CalCompensation)
    t4 = threading.Thread(target=AngleVerification)

    t1.start()
    t2.start()
    t3.start()
    t4.start()
    t1.join()
    t2.join()
    t3.join()
    t4.join()








